Strict
Import mojo
Import agfx

#Rem
Header: VERLET Physic - link - CONSTRAINT
#End


#Rem
Summary: TVerletLink class
#End
Class TVerletLink

	#Rem
	Summary: start point of constraint
	#End
	Field p1:TVerletPoint
	#Rem
	Summary: end point of constraint
	#End
	Field p2:TVerletPoint	
	
	#Rem
	Summary: distance - length bewteen P1,P2
	#End
	Field restLength:Float
	
	#Rem
	Summary: power coeficient between points
	#End
	Field k:Float	
	
	#Rem
	Summary: stress value
	#End
	Field stress:Float
	
	#Rem
	Summary: reference to verlet group
	#End
	Field parent:TVerletGroup
	
	#Rem
	Summary: constructor
	#End
	Method New( p1:TVerletPoint, p2:TVerletPoint, k:Float)
		
		Self.p1 = p1
		Self.p2 = p2		
		Self.k = k
		Self.CalcRestLength()		
		
	End

	#Rem
	Summary: static function to create link
	#End
	Function Create:TVerletLink( p1:TVerletPoint, p2:TVerletPoint, k:Float)
		Local l:TVerletLink = New TVerletLink
		l.p1 = p1
		l.p2 = p2		
		l.k = k
		l.CalcRestLength()		
		Return l
	End
	
	#Rem
	Summary: Update 	
	#End
	Method Update:Void()
	
		Local dx:Float = Self.p1.pos.x - Self.p2.pos.x
		Local dy:Float = Self.p1.pos.y - Self.p2.pos.y
		Local dist:Float = Sqrt( dx*dx + dy*dy)
		Local w:Float = Self.p1.mass + Self.p2.mass
		
		If Self.p1.active Then
			Self.p1.pos.x -= ((dx / dist) * ((dist - Self.restLength) * k)) * (Self.p1.mass * (1.0/w))
			Self.p1.pos.y -= ((dy / dist) * ((dist - Self.restLength) * k)) * (Self.p1.mass * (1.0/w))		
		Else
			Self.p1.pos.Set(Self.p1.old_pos)
		EndIf
		
		If Self.p2.active Then
			Self.p2.pos.x += ((dx / dist) * ((dist - Self.restLength) * k)) * (Self.p2.mass * (1.0/w))
			Self.p2.pos.y += ((dy / dist) * ((dist - Self.restLength) * k)) * (Self.p2.mass * (1.0/w))
		Else
			Self.p2.pos.Set(Self.p2.old_pos)
			
		EndIf
		
		Self.stress = (dist - Self.restLength) / Self.restLength
		
	End Method	
	
	#Rem
	Summary: render debug information
	#End
	Method Render:Void()
		SetColor(255,255,255)
		DrawLine(p1.pos.x,p1.pos.y, p2.pos.x,p2.pos.y)
	End Method
	
	#Rem
	Summary: calc length of points
	#End
	Method CalcRestLength:Void()				
		Self.restLength = VecDist(Self.p1.pos,Self.p2.pos)
	End Method	
End